#ifndef ARM_CONTROL__ARM_ACTION_TYPE7_HPP_
#define ARM_CONTROL__ARM_ACTION_TYPE7_HPP_

#include "arm_control/arm_action_base.hpp"
#include "fruit_detector/msg/detection_array.hpp"
#include "std_msgs/msg/int32.hpp"
#include <moveit/move_group_interface/move_group_interface.h>
#include "rclcpp_action/rclcpp_action.hpp"
#include "voice_planner/action/voice_planner.hpp"
#include <vector>
#include <limits>
#include <cmath>

namespace arm_control
{

// 水果检测数据结构
struct FruitData7 {
  int32_t type;
  float offset_x;
  float offset_y;
  bool isgood;
  
  // 计算offset (使用欧几里得距离)
  float get_offset() const {
    return std::sqrt(offset_x * offset_x + offset_y * offset_y);
  }
};

// 动作类型7：左臂xuan12关节运动到0.7853，然后移动到C_left_pose，然后根据检测数据进行抓取
class ArmActionType7 : public ArmActionBase
{
public:
  explicit ArmActionType7(rclcpp::Logger logger, rclcpp::Node::SharedPtr node);
  
  void execute(
    const std::shared_ptr<GoalHandleARM> goal_handle,
    std::shared_ptr<ARM::Feedback> feedback,
    std::shared_ptr<ARM::Result> result) override;

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Subscription<fruit_detector::msg::DetectionArray>::SharedPtr left_sub_;
  
  // 爪子发布器
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr zhuazi_left_publisher_;
  
  // MoveIt 接口
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_left_;
  
  // VoicePlanner action 客户端
  using VoicePlanner = voice_planner::action::VoicePlanner;
  rclcpp_action::Client<VoicePlanner>::SharedPtr voice_planner_client_;
  
  // 存储订阅的数据
  std::vector<FruitData7> left_data_;
  
  // 回调函数
  void left_callback(const fruit_detector::msg::DetectionArray::SharedPtr msg);
  
  // 辅助函数：根据类型数组选择offset最小的水果
  FruitData7 select_best_fruit(
    const std::vector<int32_t>& type_array,
    const std::vector<FruitData7>& data,
    bool& found);
  
  // MoveIt 运动控制函数：将机械臂末端向下移动指定距离，并根据offset调整xy位置
  bool move_down(
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group,
    double distance,
    const std::string& arm_name,
    double offset_x = 0.0,
    double offset_y = 0.0);
};

} // namespace arm_control

#endif // ARM_CONTROL__ARM_ACTION_TYPE7_HPP_

